// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:

// Geometric Tools, LLC
// Copyright (c) 1998-2010
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt

#include "U2Quaternion.h"

#include "U2Math.h"
//#include "U2Matrix3.h"
//#include "U2Vector3.h"


U2EG_NAMESPACE_USING


const u2real U2Quaternion::msEpsilon = 1e-03;
const U2Quaternion U2Quaternion::ZERO(0,0,0,0);
const U2Quaternion U2Quaternion::IDENTITY(1,0,0,0);

//-----------------------------------------------------------------------
void U2Quaternion::FromRotationMatrix (const U2Matrix3& kRot)
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "U2Quaternion Calculus and Fast Animation".

    u2real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
    u2real fRoot;

    if ( fTrace > 0.0 )
    {
        // |w| > 1/2, may as well choose w > 1/2
        fRoot = U2Math::Sqrt(fTrace + 1.0f);  // 2w
        w = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;  // 1/(4w)
        x = (kRot[2][1]-kRot[1][2])*fRoot;
        y = (kRot[0][2]-kRot[2][0])*fRoot;
        z = (kRot[1][0]-kRot[0][1])*fRoot;
    }
    else
    {
        // |w| <= 1/2
        static size_t s_iNext[3] = { 1, 2, 0 };
        size_t i = 0;
        if ( kRot[1][1] > kRot[0][0] )
            i = 1;
        if ( kRot[2][2] > kRot[i][i] )
            i = 2;
        size_t j = s_iNext[i];
        size_t k = s_iNext[j];

        fRoot = U2Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);
        u2real* apkQuat[3] = { &x, &y, &z };
        *apkQuat[i] = 0.5f*fRoot;
        fRoot = 0.5f/fRoot;
        w = (kRot[k][j]-kRot[j][k])*fRoot;
        *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
        *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
    }
}
//-----------------------------------------------------------------------
void U2Quaternion::ToRotationMatrix (U2Matrix3& kRot) const
{
    u2real fTx  = x+x;
    u2real fTy  = y+y;
    u2real fTz  = z+z;
    u2real fTwx = fTx*w;
    u2real fTwy = fTy*w;
    u2real fTwz = fTz*w;
    u2real fTxx = fTx*x;
    u2real fTxy = fTy*x;
    u2real fTxz = fTz*x;
    u2real fTyy = fTy*y;
    u2real fTyz = fTz*y;
    u2real fTzz = fTz*z;

    kRot[0][0] = 1.0f-(fTyy+fTzz);
    kRot[0][1] = fTxy-fTwz;
    kRot[0][2] = fTxz+fTwy;
    kRot[1][0] = fTxy+fTwz;
    kRot[1][1] = 1.0f-(fTxx+fTzz);
    kRot[1][2] = fTyz-fTwx;
    kRot[2][0] = fTxz-fTwy;
    kRot[2][1] = fTyz+fTwx;
    kRot[2][2] = 1.0f-(fTxx+fTyy);
}
//-----------------------------------------------------------------------
void U2Quaternion::FromAngleAxis (const Radian& rfAngle,
    const U2Vector3& rkAxis)
{
    // assert:  axis[] is unit length
    //
    // The quaternion representing the rotation is
    //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    Radian fHalfAngle ( 0.5*rfAngle );
    u2real fSin = U2Math::Sin(fHalfAngle);
    w = U2Math::Cos(fHalfAngle);
    x = fSin*rkAxis.x;
    y = fSin*rkAxis.y;
    z = fSin*rkAxis.z;
}
//-----------------------------------------------------------------------
void U2Quaternion::ToAngleAxis (Radian& rfAngle, U2Vector3& rkAxis) const
{
    // The quaternion representing the rotation is
    //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    u2real fSqrLength = x*x+y*y+z*z;
    if ( fSqrLength > 0.0 )
    {
        rfAngle = 2.0*U2Math::ACos(w);
        u2real fInvLength = U2Math::InvSqrt(fSqrLength);
        rkAxis.x = x*fInvLength;
        rkAxis.y = y*fInvLength;
        rkAxis.z = z*fInvLength;
    }
    else
    {
        // angle is 0 (mod 2*pi), so any axis will do
        rfAngle = Radian(0.0);
        rkAxis.x = 1.0;
        rkAxis.y = 0.0;
        rkAxis.z = 0.0;
    }
}
//-----------------------------------------------------------------------
void U2Quaternion::FromAxes (const U2Vector3* akAxis)
{
    U2Matrix3 kRot;

    for (size_t iCol = 0; iCol < 3; iCol++)
    {
        kRot[0][iCol] = akAxis[iCol].x;
        kRot[1][iCol] = akAxis[iCol].y;
        kRot[2][iCol] = akAxis[iCol].z;
    }

    FromRotationMatrix(kRot);
}
//-----------------------------------------------------------------------
void U2Quaternion::FromAxes (const U2Vector3& xaxis, const U2Vector3& yaxis, const U2Vector3& zaxis)
{
    U2Matrix3 kRot;

    kRot[0][0] = xaxis.x;
    kRot[1][0] = xaxis.y;
    kRot[2][0] = xaxis.z;

    kRot[0][1] = yaxis.x;
    kRot[1][1] = yaxis.y;
    kRot[2][1] = yaxis.z;

    kRot[0][2] = zaxis.x;
    kRot[1][2] = zaxis.y;
    kRot[2][2] = zaxis.z;

    FromRotationMatrix(kRot);

}
//-----------------------------------------------------------------------
void U2Quaternion::ToAxes (U2Vector3* akAxis) const
{
    U2Matrix3 kRot;

    ToRotationMatrix(kRot);

    for (size_t iCol = 0; iCol < 3; iCol++)
    {
        akAxis[iCol].x = kRot[0][iCol];
        akAxis[iCol].y = kRot[1][iCol];
        akAxis[iCol].z = kRot[2][iCol];
    }
}
//-----------------------------------------------------------------------
U2Vector3 U2Quaternion::xAxis(void) const
{
    //u2real fTx  = 2.0*x;
    u2real fTy  = 2.0f*y;
    u2real fTz  = 2.0f*z;
    u2real fTwy = fTy*w;
    u2real fTwz = fTz*w;
    u2real fTxy = fTy*x;
    u2real fTxz = fTz*x;
    u2real fTyy = fTy*y;
    u2real fTzz = fTz*z;

    return U2Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
}
//-----------------------------------------------------------------------
U2Vector3 U2Quaternion::yAxis(void) const
{
    u2real fTx  = 2.0f*x;
    u2real fTy  = 2.0f*y;
    u2real fTz  = 2.0f*z;
    u2real fTwx = fTx*w;
    u2real fTwz = fTz*w;
    u2real fTxx = fTx*x;
    u2real fTxy = fTy*x;
    u2real fTyz = fTz*y;
    u2real fTzz = fTz*z;

    return U2Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
}
//-----------------------------------------------------------------------
U2Vector3 U2Quaternion::zAxis(void) const
{
    u2real fTx  = 2.0f*x;
    u2real fTy  = 2.0f*y;
    u2real fTz  = 2.0f*z;
    u2real fTwx = fTx*w;
    u2real fTwy = fTy*w;
    u2real fTxx = fTx*x;
    u2real fTxz = fTz*x;
    u2real fTyy = fTy*y;
    u2real fTyz = fTz*y;

    return U2Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
}
//-----------------------------------------------------------------------
void U2Quaternion::ToAxes (U2Vector3& xaxis, U2Vector3& yaxis, U2Vector3& zaxis) const
{
    U2Matrix3 kRot;

    ToRotationMatrix(kRot);

    xaxis.x = kRot[0][0];
    xaxis.y = kRot[1][0];
    xaxis.z = kRot[2][0];

    yaxis.x = kRot[0][1];
    yaxis.y = kRot[1][1];
    yaxis.z = kRot[2][1];

    zaxis.x = kRot[0][2];
    zaxis.y = kRot[1][2];
    zaxis.z = kRot[2][2];
}

//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::operator+ (const U2Quaternion& rkQ) const
{
    return U2Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::operator- (const U2Quaternion& rkQ) const
{
    return U2Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::operator* (const U2Quaternion& rkQ) const
{
    // NOTE:  Multiplication is not generally commutative, so in most
    // cases p*q != q*p.

    return U2Quaternion
    (
        w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
        w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
        w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
        w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
    );
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::operator* (u2real fScalar) const
{
    return U2Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
}
/*
//-----------------------------------------------------------------------
U2Quaternion operator* (u2real fScalar, const U2Quaternion& rkQ)
{
    return U2Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
        fScalar*rkQ.z);
}
*/
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::operator- () const
{
    return U2Quaternion(-w,-x,-y,-z);
}
//-----------------------------------------------------------------------
u2real U2Quaternion::Dot (const U2Quaternion& rkQ) const
{
    return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
}
//-----------------------------------------------------------------------
u2real U2Quaternion::Norm () const
{
    return w*w+x*x+y*y+z*z;
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::Inverse () const
{
    u2real fNorm = w*w+x*x+y*y+z*z;
    if ( fNorm > 0.0 )
    {
        u2real fInvNorm = 1.0f/fNorm;
        return U2Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
    }
    else
    {
        // return an invalid result to flag the error
        return ZERO;
    }
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::UnitInverse () const
{
    // assert:  'this' is unit length
    return U2Quaternion(w,-x,-y,-z);
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::Exp () const
{
    // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
    // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

    Radian fAngle ( U2Math::Sqrt(x*x+y*y+z*z) );
    u2real fSin = U2Math::Sin(fAngle);

    U2Quaternion kResult;
    kResult.w = U2Math::Cos(fAngle);

    if ( U2Math::Abs(fSin) >= msEpsilon )
    {
        u2real fCoeff = fSin/(fAngle.valueRadians());
        kResult.x = fCoeff*x;
        kResult.y = fCoeff*y;
        kResult.z = fCoeff*z;
    }
    else
    {
        kResult.x = x;
        kResult.y = y;
        kResult.z = z;
    }

    return kResult;
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::Log () const
{
    // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
    // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

    U2Quaternion kResult;
    kResult.w = 0.0;

    if ( U2Math::Abs(w) < 1.0 )
    {
        Radian fAngle ( U2Math::ACos(w) );
        u2real fSin = U2Math::Sin(fAngle);
        if ( U2Math::Abs(fSin) >= msEpsilon )
        {
            u2real fCoeff = fAngle.valueRadians()/fSin;
            kResult.x = fCoeff*x;
            kResult.y = fCoeff*y;
            kResult.z = fCoeff*z;
            return kResult;
        }
    }

    kResult.x = x;
    kResult.y = y;
    kResult.z = z;

    return kResult;
}
//-----------------------------------------------------------------------
U2Vector3 U2Quaternion::operator* (const U2Vector3& v) const
{
	// nVidia SDK implementation
	U2Vector3 uv, uuv;
	U2Vector3 qvec(x, y, z);
	uv = qvec.crossProduct(v);
	uuv = qvec.crossProduct(uv);
	uv *= (2.0f * w);
	uuv *= 2.0f;

	return v + uv + uuv;
}
//-----------------------------------------------------------------------
bool U2Quaternion::equals(const U2Quaternion& rhs, const Radian& tolerance) const
{
    u2real fCos = Dot(rhs);
    Radian angle = U2Math::ACos(fCos);

	return (U2Math::Abs(angle.valueRadians()) <= tolerance.valueRadians())
        || U2Math::RealEqual(angle.valueRadians(), U2Math::PI, tolerance.valueRadians());
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::Slerp (u2real fT, const U2Quaternion& rkP,
    const U2Quaternion& rkQ, bool shortestPath)
{
    u2real fCos = rkP.Dot(rkQ);
    U2Quaternion rkT;

    // Do we need to invert rotation?
    if (fCos < 0.0f && shortestPath)
    {
        fCos = -fCos;
        rkT = -rkQ;
    }
    else
    {
        rkT = rkQ;
    }

    if (U2Math::Abs(fCos) < 1 - msEpsilon)
    {
        // Standard case (slerp)
        u2real fSin = U2Math::Sqrt(1 - U2Math::Sqr(fCos));
        Radian fAngle = U2Math::ATan2(fSin, fCos);
        u2real fInvSin = 1.0f / fSin;
        u2real fCoeff0 = U2Math::Sin((1.0f - fT) * fAngle) * fInvSin;
        u2real fCoeff1 = U2Math::Sin(fT * fAngle) * fInvSin;
        return fCoeff0 * rkP + fCoeff1 * rkT;
    }
    else
    {
        // There are two situations:
        // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
        //    interpolation safely.
        // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
        //    are an infinite number of possibilities interpolation. but we haven't
        //    have method to fix this case, so just use linear interpolation here.
        U2Quaternion t = (1.0f - fT) * rkP + fT * rkT;
        // taking the complement requires renormalisation
        t.normalise();
        return t;
    }
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::SlerpExtraSpins (u2real fT,
    const U2Quaternion& rkP, const U2Quaternion& rkQ, int iExtraSpins)
{
    u2real fCos = rkP.Dot(rkQ);
    Radian fAngle ( U2Math::ACos(fCos) );

    if ( U2Math::Abs(fAngle.valueRadians()) < msEpsilon )
        return rkP;

    u2real fSin = U2Math::Sin(fAngle);
    Radian fPhase ( U2Math::PI*iExtraSpins*fT );
    u2real fInvSin = 1.0f/fSin;
    u2real fCoeff0 = U2Math::Sin((1.0f-fT)*fAngle - fPhase)*fInvSin;
    u2real fCoeff1 = U2Math::Sin(fT*fAngle + fPhase)*fInvSin;
    return fCoeff0*rkP + fCoeff1*rkQ;
}
//-----------------------------------------------------------------------
void U2Quaternion::Intermediate (const U2Quaternion& rkQ0,
    const U2Quaternion& rkQ1, const U2Quaternion& rkQ2,
    U2Quaternion& rkA, U2Quaternion& rkB)
{
    // assert:  q0, q1, q2 are unit quaternions

    U2Quaternion kQ0inv = rkQ0.UnitInverse();
    U2Quaternion kQ1inv = rkQ1.UnitInverse();
    U2Quaternion rkP0 = kQ0inv*rkQ1;
    U2Quaternion rkP1 = kQ1inv*rkQ2;
    U2Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
    U2Quaternion kMinusArg = -kArg;

    rkA = rkQ1*kArg.Exp();
    rkB = rkQ1*kMinusArg.Exp();
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::Squad (u2real fT,
    const U2Quaternion& rkP, const U2Quaternion& rkA,
    const U2Quaternion& rkB, const U2Quaternion& rkQ, bool shortestPath)
{
    u2real fSlerpT = 2.0f*fT*(1.0f-fT);
    U2Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
    U2Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
    return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
}
//-----------------------------------------------------------------------
u2real U2Quaternion::normalise(void)
{
    u2real len = Norm();
    u2real factor = 1.0f / U2Math::Sqrt(len);
    *this = *this * factor;
    return len;
}
//-----------------------------------------------------------------------
Radian U2Quaternion::getRoll(bool reprojectAxis) const
{
	if (reprojectAxis)
	{
		// roll = atan2(localx.y, localx.x)
		// pick parts of xAxis() implementation that we need
//			u2real fTx  = 2.0*x;
		u2real fTy  = 2.0f*y;
		u2real fTz  = 2.0f*z;
		u2real fTwz = fTz*w;
		u2real fTxy = fTy*x;
		u2real fTyy = fTy*y;
		u2real fTzz = fTz*z;

		// U2Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);

		return Radian(U2Math::ATan2(fTxy+fTwz, 1.0f-(fTyy+fTzz)));
	}
	else
	{
		return Radian(U2Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));
	}
}
//-----------------------------------------------------------------------
Radian U2Quaternion::getPitch(bool reprojectAxis) const
{
	if (reprojectAxis)
	{
		// pitch = atan2(localy.z, localy.y)
		// pick parts of yAxis() implementation that we need
		u2real fTx  = 2.0f*x;
//			u2real fTy  = 2.0f*y;
		u2real fTz  = 2.0f*z;
		u2real fTwx = fTx*w;
		u2real fTxx = fTx*x;
		u2real fTyz = fTz*y;
		u2real fTzz = fTz*z;

		// U2Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
		return Radian(U2Math::ATan2(fTyz+fTwx, 1.0f-(fTxx+fTzz)));
	}
	else
	{
		// internal version
		return Radian(U2Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));
	}
}
//-----------------------------------------------------------------------
Radian U2Quaternion::getYaw(bool reprojectAxis) const
{
	if (reprojectAxis)
	{
		// yaw = atan2(localz.x, localz.z)
		// pick parts of zAxis() implementation that we need
		u2real fTx  = 2.0f*x;
		u2real fTy  = 2.0f*y;
		u2real fTz  = 2.0f*z;
		u2real fTwy = fTy*w;
		u2real fTxx = fTx*x;
		u2real fTxz = fTz*x;
		u2real fTyy = fTy*y;

		// U2Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));

		return Radian(U2Math::ATan2(fTxz+fTwy, 1.0f-(fTxx+fTyy)));
	}
	else
	{
		// internal version
		return Radian(U2Math::ASin(-2*(x*z - w*y)));
	}
}
//-----------------------------------------------------------------------
U2Quaternion U2Quaternion::nlerp(u2real fT, const U2Quaternion& rkP,
    const U2Quaternion& rkQ, bool shortestPath)
{
	U2Quaternion result;
    u2real fCos = rkP.Dot(rkQ);
	if (fCos < 0.0f && shortestPath)
	{
		result = rkP + fT * ((-rkQ) - rkP);
	}
	else
	{
		result = rkP + fT * (rkQ - rkP);
	}
    result.normalise();
    return result;
}

